//
// Created by infore on 2024/6/25.
//

#ifndef VISON_LOC_PREPARE_MOVE_PLANNER_H
#define VISON_LOC_PREPARE_MOVE_PLANNER_H
#include "iostream"
#include <unordered_map>
#include <string>
#include <vector>
#include <utility>
#include <utility>

using namespace std;
#define TOLERATE_X_1  8 //单位:mm
#define TOLERATE_Y_1  8 //单位:mm
#define TOLERATE_THETA_1  0.3 //单位:弧度

#define TOLERATE_X_2  2
#define TOLERATE_Y_2  1.5
#define TOLERATE_THETA_2  0.02


enum StepType {X, Y, THETA};

enum class StepStatus {
    
    SUCCESS = 1, //定位/自检成功
    OFFSET = 2, //执行偏移量结果
    SENSOR = 10,
    FAILURE_SELFCHECK = 8, //自检失败

};
//std::unordered_map<StepStatus, std::string> status_names = {
//        {StepStatus::SUCCESS, "SUCCESS"},
//        {StepStatus::OFFSET, "OFFSET"},
//        {StepStatus::SENSOR, "SENSOR"},
//        {StepStatus::FAILURE_SELFCHECK, "FAILURE_SELFCHECK"},
//};


struct Step {
    explicit Step(float x = 0.0, float y =0.0, float theta = 0.0) {
        this->x = x;
        this->y = y;
        this->theta = theta;
    };


    StepStatus status = StepStatus::OFFSET; //状态码
    float x = 0.0;
    float y = 0.0;
    float theta = 0.0;

    friend ostream& operator<<(ostream& os, const Step& step) {
        os << "Step(status:" << static_cast<int>(step.status)
                             << ", x:" << step.x << ", y:" << step.y
                             << ", theta:" << step.theta << ")";
        return os;
    };
};

struct HisStep {
//    int x = 0;       //X轴连续调整次数
//    int y = 0;       //Y轴连续调整次数
//    int theta = 0;   //Theta轴连续调整次数
    unordered_map<StepType, int> hisMap = {{X, 0}, {Y, 0}, {THETA, 0}};
    StepType cur = THETA;
};


/**
 *只按X轴移动
 * @param oriStep
 * @param hisStep
 * @param step
 * @param thetathrehold
 * @return
 */
bool getX(Step& oriStep, HisStep& hisStep, Step& step, float thetathrehold);
/**
 * 只按Y轴移动
 * @param oriStep
 * @param hisStep
 * @param step
 * @param thetathrehold
 * @return
 */
bool getY(Step& oriStep, HisStep& hisStep, Step& step, float thetathrehold);
/**
 * 只转动舵轮调整车体角度
 * @param oriStep
 * @param hisStep
 * @param step
 * @param thetathrehold
 * @return
 */
bool getTHETA(Step& oriStep, HisStep& hisStep, Step& step, float thetathrehold);
/**
 * 控制策略：基于车体坐标系，先对齐(坐标轴平行)后靠近
 * 因为车体按坐标轴移动，拆分动作控制
 * x轴正方向指向车头前进方向，有正方向指向符合右手定则，逆时针转动角度为正
 * @param x
 * @param y
 * @param theta
 * @return
 */
Step getStep(float x, float y, float theta, HisStep& hisStep);

#endif //VISON_LOC_PREPARE_MOVE_PLANNER_H
